#ifndef _VRAT_H_
#define _VRAT_H_

#include <ros/ros.h>
#include <sensor_msgs/CompressedImage.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <geometry_msgs/Pose2D.h>

#include <opencv2/highgui/highgui.hpp>

#include <irat_msgs/IRatVelocity.h>
#include <irat_msgs/IRatDistance.h>
#include <irat_msgs/IRatRangers.h>
#include <irat_msgs/IRatRanger.h>
#include <irat_msgs/IRatCameraConfig.h>
#include <irat_msgs/IRatPower.h>

#include <Stage-4.1/stage.hh>
#include <Stage-4.1/option.hh>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>
using boost::property_tree::ptree;

#include <cmath>
#include <string>

#include "ratcamera.h"
#include "ply.h"
#include "3ds.h"
#include "light.h"

class vRat : public Stg::ModelPosition
{
	
public:

	vRat(Stg::World *world, std::string& colour, ptree& config);
	virtual ~vRat();

	virtual void Update();
	virtual void Move();
	virtual void DrawBlocks();
	Light *sun;

private:
	
	std::string colour;

	// Stage interface
	Stg::ModelRanger *stg_ranger;
	RatCamera *stg_cam;

	// ROS interface
	ros::NodeHandle *nh;
	std_msgs::Header header;
	ros::Publisher *p_cam, *p_cam_raw, *p_odom, *p_pose, *p_ranger, *p_power;
	ros::Subscriber *s_dist, *s_vel;
	ros::Time sub_stamp;
	void callback_vel(const irat_msgs::IRatVelocityConstPtr& msg);
	void callback_dist(const irat_msgs::IRatDistanceConstPtr& msg);
	sensor_msgs::CompressedImage ros_img;
	sensor_msgs::Image ros_img_raw;
	irat_msgs::IRatRangers ros_ranger;
	bool draw_ir_beams;
	ModelPLY *ply;

	float stall_av;

	void load_bitmap(std::string fname)
	{
		// load bitmap from file and turn into blocks
		std::vector<Stg::rotrect_t> rects;
		if(Stg::rotrects_from_image_file(fname, rects))
		{
			printf("failed to load rects from image file \"%s\"", fname.c_str());
			return;
		}

		FOR_EACH(rect, rects)
		{
			std::vector<Stg::point_t> pts(4);

			const double x = rect->pose.x;
			const double y = rect->pose.y;
			const double w = rect->size.x;
			const double h = rect->size.y;

			pts[0].x = x;
			pts[0].y = y;
			pts[1].x = x + w;
			pts[1].y = y;
			pts[2].x = x + w;
			pts[2].y = y + h;
			pts[3].x = x;
			pts[3].y = y + h;

			blockgroup.AppendBlock(new Stg::Block(this, pts, 0, 1, color, true, false));
		}

		blockgroup.CalcSize();

		UnMapWithChildren(0);
		UnMapWithChildren(1);
		MapWithChildren(0);
		MapWithChildren(1);
	}

};

#endif
